/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import button.LatchWhenPressedButton;
import button.SwitchWhenPressedButton;
import button.SwitchUntilReleasedButton;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO.EnhancedIOException;
import edu.wpi.first.wpilibj.Joystick;
import file.AutonomousScript;
import file.Configuration;
import file.DataRecorder;

/**
 *
 * @author Mentors  Carl Burger
 */
public class DriverStation708 {

    // joystick ports
    final int kDriveStick1 = 1;  // port 1 on driver station
    final int kOperatorStick1 = 2;  // port 2 on driver station
    final int kOperatorStick2 = 3;  // port 3 on driver station
    // button channels
    // driver "stick 1" buttons
//    final int kSelectGridPole     = 1; // switches between Left Center Right
    final int kInvertAxes = 2;
    final int kRelativeSteering = 3;
    final int kResetGyro = 6;
    final int kFindLineToLeft = 7;
    final int kFindLineToRight = 8;
    final int kEncodersReset = 11;
    // driver "stick 2" buttons
    final int kHeadingTo0 = 1;
//    final int kNudgeCloser        = 2; // move the robot slowly in one direction
//    final int kNudgeFarther       = 3;
//    final int kNudgeLeft          = 4;
//    final int kNudgeRight         = 5;
    final int kDriveControl = 6; // switches between 1 or 2 joysticks
    final int kReadConfig = 2; // reads the config file again (disabled only)
    final int kGripperToggle = 3;
    final int kElevatorFeeder = 7;
    final int kElevatorFloor = 8;
    final int kElevatorBottomPeg = 9;
    final int kElevatorMiddlePeg = 10;
    final int kElevatorTopPeg = 11;
    // operator stick buttons
    // elevator up and down
    // wrist up and down
    // gripper open and close
    final int kUnused = 1;
    final int kElevatorBottomPegPlus = 2;
    final int kElevatorMiddlePegPlus = 3;
    final int kElevatorTopPegPlus = 4;
    final int kUnusedOpButton5 = 5;
    final int kDeployMinibot2 = 6;
    final int kScreenControl = 7;  // toggles Message Center updates on/off
    final int kPrevScreen = 8;  // displays previous Message Center screen
    final int kNextScreen = 9;  // displays next Message Center screen
    final int kPrevAutoMode = 10; // selects previous autonomous mode
    final int kNextAutoMode = 11; // selects next autonomous mode
    // driver station enhanced I/O digital inputs for buttons
    // digital inputs
    // Channel/State   Channel/State   Function
    //    4    false      1    false   Bottom Peg; outer
    //    4    false      2    false   Middle Peg; outer
    //    4    false      3    false   Top Peg; outer
    //    4    true       1    false   Bottom Peg; center
    //    4    true       2    false   Middle Peg; center
    //    4    true       3    false   Top Peg; center
    final int kBottomPegPosition = 1;
    final int kMiddlePegPosition = 2;
    final int kTopPegPosition = 3;
    final int kBottomPegPlusPosition = 1;
    final int kMiddlePegPlusPosition = 2;
    final int kTopPegPlusPosition = 3;
    final int kModifier = 4;
    final int kFloorPosition = 5;
    final int kFeederPosition = 6;
    final int kHangAndRelease = 7;
    final int kDeployMinibot = 8;
    // digital outputs    
    final int kBottomPegIndicator = 9;
    final int kMiddlePegIndicator = 10;
    final int kTopPegIndicator = 11;
    final int kBottomPegPlusIndicator = 12;
    final int kMiddlePegPlusIndicator = 13;
    final int kTopPegPlusIndicator = 14;
    final int kFloorIndicator = 15;
    final int kFeederIndicator = 16;
    // analog inputs - subtract 16 for actual analog input channel
    final int kTriangleLEDs = 17;  // analog input channel 1
    final int kSquareLEDs = 18;
    final int kCircleLEDs = 19;
    final int kWristDown = 20;
    final int kWristUp = 21;
    final int kWristLow = 22;
    final int kWristMedium = 23;
    final int kWristHigh = 24;  // analog input channel 8
    // driver station input
    DriverStation driverStation;
    DriverStationEnhancedIO driverStationIO;
    Gamepad driveStick1;       // driver input
    Gamepad operatorStick1;    // operator stick for wrist input
    Gamepad operatorStick2;    // operator stick for elevator input
    Joystick dummy;
    //joysticks
    SwitchWhenPressedButton relativeSteeringButton; // change orientation of driver relative steering
    LatchWhenPressedButton readConfigButton;       // Reads config file again (disabled only)
    LatchWhenPressedButton resetGyroButton;        // Resets the gyro to 0 degrees
    LatchWhenPressedButton findLineToLeftButton;
    LatchWhenPressedButton findLineToRightButton;
    LatchWhenPressedButton encodersResetButton;    // resets encoders for debugging
    LatchWhenPressedButton headingTo0Button;       // rotate robot to 0 degrees
//    SwitchUntilReleasedButton nudgeCloserButton;      // nudge the robot closer to the driver station
//    SwitchUntilReleasedButton nudgeFartherButton;     // nudge the robot farther from the driver station
//    SwitchUntilReleasedButton nudgeLeftButton;        // nudge the robot left
//    SwitchUntilReleasedButton nudgeRightButton;       // nudge the robot right
    SwitchWhenPressedButton driveControlButton;     // toggle 1- or 2- drive joysticks
    LatchWhenPressedButton nextScreenButton;      // display next Message Center screen
    LatchWhenPressedButton prevScreenButton;      // display previous Message Center screen
    LatchWhenPressedButton screenControlButton;   // enable/disable Message Center screen updates
    LatchWhenPressedButton nextAutoModeButton;    // select next autonomous mode
    LatchWhenPressedButton prevAutoModeButton;    // select previous autonomous mode
    // *********** Temporary manipulator control from the joysticks ****************
    LatchWhenPressedButton bottomPegButtonTemp;       // move elevator to top peg;    outer grid posts
    LatchWhenPressedButton middlePegButtonTemp;       // move elevator to middle peg; outer grid posts
    LatchWhenPressedButton topPegButtonTemp;          // move elevator to top peg;    outer grid posts
    LatchWhenPressedButton bottomPegPlusButtonTemp;   // move elevator to top peg;    center grid post
    LatchWhenPressedButton middlePegPlusButtonTemp;   // move elevator to middle peg; center grid post
    LatchWhenPressedButton topPegPlusButtonTemp;      // move elevator to top peg;    center grid post
    LatchWhenPressedButton floorButtonTemp;           // move elevator to floor
    LatchWhenPressedButton feederButtonTemp;          // move elevator to feeder
    SwitchWhenPressedButton gripperToggleButton;      //open/close the grippere
    // *********** End of temporary elevator control from the joysticks ****************
    // driver station enhanced I/O
    LatchWhenPressedButton bottomPegButton;       // move elevator to top peg;    outer grid posts
    LatchWhenPressedButton middlePegButton;       // move elevator to middle peg; outer grid posts
    LatchWhenPressedButton topPegButton;          // move elevator to top peg;    outer grid posts
    LatchWhenPressedButton bottomPegPlusButton;   // move elevator to top peg;    center grid post
    LatchWhenPressedButton middlePegPlusButton;   // move elevator to middle peg; center grid post
    LatchWhenPressedButton topPegPlusButton;      // move elevator to top peg;    center grid post
    LatchWhenPressedButton floorButton;           // move elevator to floor
    LatchWhenPressedButton feederButton;          // move elevator to feeder
    LatchWhenPressedButton hangAndReleaseButton;  // release tube on peg and retract
    LatchWhenPressedButton deployMinibotButton;   // release minibot to climb a tower
    SwitchUntilReleasedButton triangleButton;
    SwitchUntilReleasedButton circleButton;
    SwitchUntilReleasedButton squareButton;
    SwitchUntilReleasedButton wristDownButton;       // move wrist down while switch is pressed
    SwitchUntilReleasedButton wristUpButton;         // move wrist up while switch is pressed
    LatchWhenPressedButton wristLowButton;        // adjust wrist
    LatchWhenPressedButton wristMediumButton;     // adjust wrist
    LatchWhenPressedButton wristHighButton;       // adjust wrist

    //variables used for storing any joystick's values
    double xAxis;
    double yAxis;
    double zAxis;

    double magnitude;
    double direction;

    // maintain previous values so that the driver station is only updated if
    // the displayed information changes
    double previousDriveStickXaxis = -2; // driver inputs
    double previousDriveStickYaxis = -2;
    double previousDriveStickZaxis = -2;
    double previousOperatorStick1Xaxis = -2; // operator stick 1 inputs
    double previousOperatorStick1Yaxis = -2;
    double previousOperatorStick2Xaxis = -2; // operator stick 2 inputs
    double previousOperatorStick2Yaxis = -2;
    String previousDriveMode = " ";
    MessageCenter messageCenter;   // Driver Station Output - 21 characters per line
    DataRecorder dataRecorder; // Telemetry data recorder
    AutonomousScript autoScript;   // Autonomous mode script
    Configuration config;       // Configuration file handler
    // objects to control
    Camera camera;
    Drivetrain drivetrain;
    DriveController driveController;
    Elevator elevator;
    Gripper gripper;
    Manipulator manipulator;
    Minibot minibot;
    Wrist wrist;
    // sensors
    Gyroscope gyroscope;
    // indicator channels
    ShapeIndicatorLEDs shapeIndicatorLEDs;
    boolean lastGripperState = false;

    public DriverStation708() {

        // instantiate all objects

        driverStation = DriverStation.getInstance();
        driverStationIO = driverStation.getEnhancedIO();

        try {
            // digital inputs for the buttons must be pulled up because the PSOC sometimes loses these settings
            // and the inputs are somehow set to floating which makes it look like buttons are randomly being pressed
            driverStationIO.setDigitalConfig(kBottomPegPosition, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
            driverStationIO.setDigitalConfig(kMiddlePegPosition, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
            driverStationIO.setDigitalConfig(kTopPegPosition, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
            driverStationIO.setDigitalConfig(kModifier, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
            driverStationIO.setDigitalConfig(kFloorPosition, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
            driverStationIO.setDigitalConfig(kFeederPosition, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
            driverStationIO.setDigitalConfig(kHangAndRelease, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
            driverStationIO.setDigitalConfig(kDeployMinibot, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);

            // digital outputs for the LED indicators on the driver station
            driverStationIO.setDigitalConfig(kBottomPegIndicator, DriverStationEnhancedIO.tDigitalConfig.kOutput);
            driverStationIO.setDigitalConfig(kMiddlePegIndicator, DriverStationEnhancedIO.tDigitalConfig.kOutput);
            driverStationIO.setDigitalConfig(kTopPegIndicator, DriverStationEnhancedIO.tDigitalConfig.kOutput);
            driverStationIO.setDigitalConfig(kBottomPegPlusIndicator, DriverStationEnhancedIO.tDigitalConfig.kOutput);
            driverStationIO.setDigitalConfig(kMiddlePegPlusIndicator, DriverStationEnhancedIO.tDigitalConfig.kOutput);
            driverStationIO.setDigitalConfig(kTopPegPlusIndicator, DriverStationEnhancedIO.tDigitalConfig.kOutput);
            driverStationIO.setDigitalConfig(kFloorIndicator, DriverStationEnhancedIO.tDigitalConfig.kOutput);
            driverStationIO.setDigitalConfig(kFeederIndicator, DriverStationEnhancedIO.tDigitalConfig.kOutput);
        } catch (EnhancedIOException e) {
            System.out.println("Driver Station button or LED configuration failed");
        }

        // Driver Station Input
        Gamepad gamepad = new Gamepad(1);
        driveStick1 = gamepad;
        operatorStick1 = gamepad;
        operatorStick2 = gamepad;   // currently not used for driving
        dummy = new Joystick(2);
        


        // Joystick and Driver Station Buttons

        relativeSteeringButton = new SwitchWhenPressedButton(dummy, kRelativeSteering,
                "Relative steering button", true);
        readConfigButton = new LatchWhenPressedButton(dummy, kReadConfig,
                "Read Config file button");
        resetGyroButton = new LatchWhenPressedButton(dummy, kResetGyro,
                "Reset gyro button");
        findLineToLeftButton = new LatchWhenPressedButton(dummy, kFindLineToLeft,
                "Find line to left button");
        findLineToRightButton = new LatchWhenPressedButton(dummy, kFindLineToRight,
                "Find line to right button");
        encodersResetButton = new LatchWhenPressedButton(dummy, kEncodersReset,
                "Encoders button");
        headingTo0Button = new LatchWhenPressedButton(dummy, kHeadingTo0,
                "Heading to 0 button");
//        nudgeCloserButton   = new SwitchUntilReleasedButton(driveStickRotate, kNudgeCloser,
//                                                             "Nudge closer button");
//        nudgeFartherButton  = new SwitchUntilReleasedButton(driveStickRotate, kNudgeFarther,
//                                                             "Nudge farther button");
//        nudgeLeftButton     = new SwitchUntilReleasedButton(driveStickRotate, kNudgeLeft,
//                                                             "Nudge left button");
//        nudgeRightButton    = new SwitchUntilReleasedButton(driveStickRotate, kNudgeRight,
//                                                             "Nudge right button");
        driveControlButton = new SwitchWhenPressedButton(dummy, kDriveControl,
                "Drive control button");

        nextScreenButton = new LatchWhenPressedButton(dummy, kNextScreen,
                "Next screen button");
        prevScreenButton = new LatchWhenPressedButton(dummy, kPrevScreen,
                "Prev screen button");
        screenControlButton = new LatchWhenPressedButton(dummy, kScreenControl,
                "Screen control button");

        nextAutoModeButton = new LatchWhenPressedButton(dummy, kNextAutoMode,
                "Next auto mode button");
        prevAutoModeButton = new LatchWhenPressedButton(dummy, kPrevAutoMode,
                "Prev auto mode button");


        // *********** Temporary manipulator control from the joysticks ****************

        bottomPegButtonTemp = new LatchWhenPressedButton(dummy, kElevatorBottomPeg,
                "Bottom peg (outer) button");
        middlePegButtonTemp = new LatchWhenPressedButton(dummy, kElevatorMiddlePeg,
                "Middle peg (outer) button");
        topPegButtonTemp = new LatchWhenPressedButton(dummy, kElevatorTopPeg,
                "Top peg (outer) button");
        bottomPegPlusButtonTemp = new LatchWhenPressedButton(dummy, kElevatorBottomPegPlus,
                "Bottom peg (center) button");
        middlePegPlusButtonTemp = new LatchWhenPressedButton(dummy, kElevatorMiddlePegPlus,
                "Middle peg (center) button");
        topPegPlusButtonTemp = new LatchWhenPressedButton(dummy, kElevatorTopPegPlus,
                "Top peg (center) button");
        floorButtonTemp = new LatchWhenPressedButton(dummy, kElevatorFloor,
                "Floor button");
        feederButtonTemp = new LatchWhenPressedButton(dummy, kElevatorFeeder,
                "Feeder button");

        gripperToggleButton = new SwitchWhenPressedButton(operatorStick2,Gamepad.button_X,
                "Gripper toggle button", false);

        // *********** End of temporary elevator control from the joysticks ****************


        //    4    false      1    false   Outer Columns, Bottom Peg
        //    4    false      2    false   Outer Middle
        //    4    false      3    false   Outer Top
        //    4    true       1    false   Center Bottom
        //    4    true       2    false   Center Middle
        //    4    true       3    false   Center Top


        bottomPegButton = new LatchWhenPressedButton(driverStationIO, kModifier, false,
                kBottomPegPosition,
                "Bottom peg (outer) button");
        middlePegButton = new LatchWhenPressedButton(driverStationIO, kModifier, false,
                kMiddlePegPosition,
                "Middle peg (outer) button");
        topPegButton = new LatchWhenPressedButton(driverStationIO, kModifier, false,
                kTopPegPosition,
                "Top peg (outer) button");

        bottomPegPlusButton = new LatchWhenPressedButton(driverStationIO, kModifier, true,
                kBottomPegPlusPosition,
                "Bottom peg (center) button");
        middlePegPlusButton = new LatchWhenPressedButton(driverStationIO, kModifier, true,
                kMiddlePegPlusPosition,
                "Middle peg (center) button");
        topPegPlusButton = new LatchWhenPressedButton(driverStationIO, kModifier, true,
                kTopPegPlusPosition,
                "Top peg (center) button");

        floorButton = new LatchWhenPressedButton(driverStationIO, kFloorPosition,
                "Floor button");
        feederButton = new LatchWhenPressedButton(driverStationIO, kFeederPosition,
                "Feeder button");

        hangAndReleaseButton = new LatchWhenPressedButton(driverStationIO, kHangAndRelease,
                "Hang & release button");
        deployMinibotButton = new LatchWhenPressedButton(driverStationIO, kDeployMinibot,
                "Deploy minibot button");

        triangleButton = new SwitchUntilReleasedButton(driverStationIO, kTriangleLEDs,
                "Triangle button");

        circleButton = new SwitchUntilReleasedButton(driverStationIO, kCircleLEDs,
                "Circle button");

        squareButton = new SwitchUntilReleasedButton(driverStationIO, kSquareLEDs,
                "Square button");

        wristDownButton = new SwitchUntilReleasedButton(driverStationIO, kWristDown,
                "Wrist down button");
        wristUpButton = new SwitchUntilReleasedButton(driverStationIO, kWristUp,
                "Wrist up button");
        wristLowButton = new LatchWhenPressedButton(driverStationIO, kWristLow,
                "Wrist low button");
        wristMediumButton = new LatchWhenPressedButton(driverStationIO, kWristMedium,
                "Wrist medium button");
        wristHighButton = new LatchWhenPressedButton(driverStationIO, kWristHigh,
                "Wrist high button");


        // get instance of other objects to control

        shapeIndicatorLEDs = ShapeIndicatorLEDs.getInstance();
        camera = Camera.getInstance();
        drivetrain = Drivetrain.getInstance();
        driveController = DriveController.getInstance();
        elevator = Elevator.getInstance();
        gripper = Gripper.getInstance();
        gyroscope = Gyroscope.getInstance();
        manipulator = Manipulator.getInstance();
        minibot = Minibot.getInstance();
        wrist = Wrist.getInstance();

        autoScript = AutonomousScript.getInstance();
        config = Configuration.getInstance();
        dataRecorder = DataRecorder.getInstance();
        messageCenter = MessageCenter.getInstance();

    }
    /**
     * processOperatorControls is called from disabledPeriodic and
     * teleopPeriodic to process driver station inputs
     */
    public void processOperatorControls(boolean disabled) {
        checkDriveControls();
       
        // check if a different screen was selected or if message center was enabled/disabled
        checkMessageCenterControls();

        checkShapeSelectorControls();

        if (encodersResetButton.getButtonState() == true) {  // used to debug encoders
            drivetrain.resetEncoders();
        }

        if (resetGyroButton.getButtonState() == true) {  // used to reset the gyro
            gyroscope.reset();
        }

        if (disabled == true) {

            if (readConfigButton.getButtonState() == true) {
                config.readConfig();      // read Config file again after changes
            }

            checkAutoModeControls();     // check if operator is changing autonomous mode

            driveController.drive(0, 0, 0, 0, 0, true);

        } else {

            if (headingTo0Button.getButtonState() == true) {

                //driveController.setHeading(0);
            }

            if (findLineToLeftButton.getButtonState() == true) {
                // driver's left is the robot's right
                driveController.findLine(DriveController.kRight);
            }

            if (findLineToRightButton.getButtonState() == true) {
                // driver's right is the robot's left
                driveController.findLine(DriveController.kLeft);
            }

            checkManipulatorControls();

            camera.move(operatorStick1.getX(), operatorStick1.getY());

            minibot.checkDeployment(deployMinibotButton.getButtonState());
        }
    }

    private void checkDriveControls() {

        double driveStickZaxis;
        double driveStickXaxis;
        double driveStickYaxis;

        driveStickXaxis = driveStick1.getXGamepad();
        driveStickYaxis = driveStick1.getYGamepad();

//        if (driveControlButton.getButtonState() == false) {
//
        driveStickZaxis = driveStick1.getZGamepad();
//        }
//        else {
//            driveStickZaxis = -driveStick2.getX();
//        }



        double magnitude = driveStick1.getMagnitude();
        double direction = driveStick1.getDirectionDegrees();

        boolean driverRelativeSteering = relativeSteeringButton.getButtonState();

        // when disabled, update joystick movement on Driver Joystick screen
        // when enabled, drive the robot
        driveController.drive(driveStickXaxis, driveStickYaxis, driveStickZaxis,
                magnitude, direction, driverRelativeSteering);
//        drivetrain.operatorDrive(driveStickXaxis, driveStickYaxis, driveStickZaxis,
//                                 magnitude, direction, driverRelativeSteering);
    }

    private void turnOnIndicator(int channel) {

        try {
            turnOffAllIndicators();
            driverStationIO.setDigitalOutput(channel, true);
        } catch (EnhancedIOException e) {
            System.out.println("turnOnIndicator " + e.getMessage());
        }
    }

    private void turnOffAllIndicators() {

        try {
            driverStationIO.setDigitalOutput(kBottomPegIndicator, false);
            driverStationIO.setDigitalOutput(kMiddlePegIndicator, false);
            driverStationIO.setDigitalOutput(kTopPegIndicator, false);
            driverStationIO.setDigitalOutput(kBottomPegPlusIndicator, false);
            driverStationIO.setDigitalOutput(kMiddlePegPlusIndicator, false);
            driverStationIO.setDigitalOutput(kTopPegPlusIndicator, false);
            driverStationIO.setDigitalOutput(kFloorIndicator, false);
            driverStationIO.setDigitalOutput(kFeederIndicator, false);
        } catch (EnhancedIOException e) {
            System.out.println("turnOffIndicators " + e.getMessage());
        }
    }

    private void checkManipulatorControls() {


        if ((floorButton.getButtonState() == true) || (floorButtonTemp.getButtonState() == true)) {
            turnOnIndicator(kFloorIndicator);
            manipulator.moveToFloor();    // moves wrist after elevator is in position
//            elevator.moveToFloor();
//            wrist.pickup();
//            gripper.release();

        }
        if ((feederButton.getButtonState() == true) || (feederButtonTemp.getButtonState() == true)) {
            elevator.moveToFeeder();
            wrist.feed();
            gripper.release();
            turnOnIndicator(kFeederIndicator);
        }
        if ((bottomPegButton.getButtonState() == true) || (bottomPegButtonTemp.getButtonState() == true)) {
            elevator.moveToBottomPeg(false);
            wrist.hang();
            turnOnIndicator(kBottomPegIndicator);
        }
        if ((middlePegButton.getButtonState() == true) || (middlePegButtonTemp.getButtonState() == true)) {
            elevator.moveToMiddlePeg(false);
            wrist.hang();
            turnOnIndicator(kMiddlePegIndicator);
        }
        if ((topPegButton.getButtonState() == true) || (topPegButtonTemp.getButtonState() == true)) {
            elevator.moveToTopPeg(false);
            wrist.hang();
            turnOnIndicator(kTopPegIndicator);
        }
        if ((bottomPegPlusButton.getButtonState() == true) || (bottomPegPlusButtonTemp.getButtonState() == true)) {
            elevator.moveToBottomPeg(true);
            wrist.hang();
            turnOnIndicator(kBottomPegPlusIndicator);
        }
        if ((middlePegPlusButton.getButtonState() == true) || (middlePegPlusButtonTemp.getButtonState() == true)) {
            elevator.moveToMiddlePeg(true);
            wrist.hang();
            turnOnIndicator(kMiddlePegPlusIndicator);
        }
        if ((topPegPlusButton.getButtonState() == true) || (topPegPlusButtonTemp.getButtonState() == true)) {
            elevator.moveToTopPeg(true);
            wrist.hang();
            turnOnIndicator(kTopPegPlusIndicator);
        }

        // check if elevator is being manually controlled (for debugging)
        elevator.move(operatorStick2.getElevator());

        // gripper toggle button to override the presets        
        if (gripperToggleButton.getButtonState() != lastGripperState) {
            gripper.toggle();
            lastGripperState = gripperToggleButton.getButtonState();
//            gripper.grasp();

//            if(gripper.hasShape()){
//                gripper.release();
//            }else{
//                gripper.grasp();
//            }
//        }
//        else {
//            gripper.grasp();
//            gripper.release();
        }


        // wrist only buttons to override the presets

        if (wristDownButton.getButtonState() == true) {
            wrist.lower();
        }
        if (wristUpButton.getButtonState() == true) {
            wrist.raise();
        }
        if (wristLowButton.getButtonState() == true) {
            wrist.pickup();
        }
        if (wristMediumButton.getButtonState() == true) {
            wrist.feed();
        }
        if (wristHighButton.getButtonState() == true) {
            wrist.hang();
        }

        if (hangAndReleaseButton.getButtonState() == true) {
            //elevator.hang();
            gripper.release();
            wrist.release();
        }

        // check if wrist is being manually controlled (for debugging)
        wrist.move(operatorStick1.getWrist());
    }

    private void checkShapeSelectorControls() {

        if (gripper.hasShape() == false) {

            shapeIndicatorLEDs.set(triangleButton.getButtonState(),
                    circleButton.getButtonState(),
                    squareButton.getButtonState());
        } else {

            shapeIndicatorLEDs.set(false, false, false);
        }
    }

    /**
     * checkMessageCenterRequests is called to process operator requests to
     * enable/disable the Message Center and to change screens
     */
    private void checkMessageCenterControls() {

        if (screenControlButton.getButtonState() == true) {
            messageCenter.toggleScreenControl();
        }

        if (nextScreenButton.getButtonState() == true) {
            messageCenter.nextScreenNumber();
        }

        if (prevScreenButton.getButtonState() == true) {
            messageCenter.prevScreenNumber();
        }
    }

    /**
     * checkAutoModeControls is called to see if the operator
     * is changing autonomous mode using the joystick
     */
    private void checkAutoModeControls() {

        if (nextAutoModeButton.getButtonState() == true) {
            autoScript.nextMode();
        }

        if (prevAutoModeButton.getButtonState() == true) {
            autoScript.prevMode();
        }
    }

    /**
     * checkDriveModeControls is called to display whether the gyro
     * is being used for relative steering.
     */
    private void displayDriveMode() {

        boolean relativeSteeringButtonState = relativeSteeringButton.getButtonState();
        String driveMode = relativeSteeringButtonState ? "Gyro on" : "Gyro off";

        if (!previousDriveMode.equals(driveMode)) {

            messageCenter.println(MessageCenter.kConfigScreen, MessageCenter.kLine3,
                    "Drive Mode: " + driveMode);
            previousDriveMode = driveMode;
        }
    }

    /**
     * displayDriverButtonPresses is called to display the number of the button
     * pressed on the Message Center's Driver Joystick screen for mapping
     * buttons on a joystick that is not labeled
     */
    private void displayDriverButtonPresses() {

        boolean buttonState;
        StringBuffer driver = new StringBuffer(MessageCenter.kLineLength);

        // Driver Joystick screen (buttons)
        for (int btn = 1; btn <= 12; btn++) {
            buttonState = driveStick1.getRawButton(btn);
            if (buttonState == true) {
                driver.append(btn).append(" ");
            }
        }

        if (driver.length() > 0) {
            messageCenter.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine6,
                    driver.toString() + " pressed");
        }
    }

    private void displayDriverInputs() {
        xAxis = driveStick1.getX();
        yAxis = driveStick1.getY();
        zAxis = driveStick1.getZ();

        magnitude = driveStick1.getMagnitude();
        direction = driveStick1.getDirectionDegrees();

        if ((previousDriveStickXaxis != xAxis)
                || (previousDriveStickYaxis != yAxis)
                || (previousDriveStickZaxis != zAxis)) {

            messageCenter.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine2,
                    "X " + messageCenter.formatDouble(xAxis, 6)
                    + " Y " + messageCenter.formatDouble(yAxis, 6));

            messageCenter.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine3,
                    "Z " + messageCenter.formatDouble(zAxis, 6));

            messageCenter.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine4,
                    "M " + messageCenter.formatDouble(magnitude, 6)
                    + " D " + messageCenter.formatDouble(direction, 6));

            previousDriveStickXaxis = xAxis;
            previousDriveStickYaxis = yAxis;
            previousDriveStickZaxis = zAxis;
        }
    }

    /**
     * displayOperatorInputs is called to display operator joystick and button
     * presses on the Message Center's Operator Joystick screen
     */
    private void displayOperatorInputs() {

        //output for first operator stick

        double Xaxis = operatorStick1.getX();
        double Yaxis = operatorStick1.getY();

        double direction = operatorStick1.getDirectionDegrees();
        double magnitude = operatorStick1.getMagnitude();

        if ((previousOperatorStick1Xaxis != Xaxis)
                || (previousOperatorStick1Yaxis != Yaxis)) {

            messageCenter.println(MessageCenter.kOperatorJoystickScreen, MessageCenter.kLine2,
                    "X " + messageCenter.formatDouble(Xaxis, 5)
                    + " Y " + messageCenter.formatDouble(Yaxis, 5));
            messageCenter.println(MessageCenter.kOperatorJoystickScreen, MessageCenter.kLine3,
                    "M " + messageCenter.formatDouble(magnitude, 5)
                    + " D " + messageCenter.formatDouble(direction, 5));

            //output joystick movement to wrist screen also, for testing
            messageCenter.println(MessageCenter.kSensorsScreen, MessageCenter.kLine6,
                    "X " + messageCenter.formatDouble(Xaxis, 5)
                    + " Y " + messageCenter.formatDouble(Yaxis, 5));

            previousOperatorStick1Xaxis = Xaxis;
            previousOperatorStick1Yaxis = Yaxis;
        }

        //output for 2nd operator stick

        Xaxis = operatorStick2.getX();
        Yaxis = operatorStick2.getY();

        direction = operatorStick2.getDirectionDegrees();
        magnitude = operatorStick2.getMagnitude();

        if ((previousOperatorStick2Xaxis != Xaxis)
                || (previousOperatorStick2Yaxis != Yaxis)) {

            messageCenter.println(MessageCenter.kOperatorJoystickScreen, MessageCenter.kLine5,
                    "X " + messageCenter.formatDouble(Xaxis, 5)
                    + " Y " + messageCenter.formatDouble(Yaxis, 5));
            messageCenter.println(MessageCenter.kOperatorJoystickScreen, MessageCenter.kLine6,
                    "M " + messageCenter.formatDouble(magnitude, 5)
                    + " D " + messageCenter.formatDouble(direction, 5));

            //output joystick movement to the elevator screen also, for testing
            messageCenter.println(MessageCenter.kSensorsScreen, MessageCenter.kLine3,
                    "X " + messageCenter.formatDouble(Xaxis, 5)
                    + " Y " + messageCenter.formatDouble(Yaxis, 5));

            previousOperatorStick2Xaxis = Xaxis;
            previousOperatorStick2Yaxis = Yaxis;
        }

        //collect button press data for operator stick 1

        boolean buttonState;
        StringBuffer opstick1buttons = new StringBuffer(MessageCenter.kLineLength);

        for (int btn = 1; btn <= 12; btn++) {
            buttonState = operatorStick1.getRawButton(btn);
            if (buttonState == true) {
                opstick1buttons.append(btn).append(" ");
            }
        }
        
        
        //collect button press data for operator stick 2
        
        StringBuffer opstick2buttons = new StringBuffer(MessageCenter.kLineLength);

        for (int btn = 1; btn <= 12; btn++) {
            buttonState = operatorStick2.getRawButton(btn);
            if (buttonState == true) {
                opstick2buttons.append(btn).append(" ");
            }
        }

        //output button press data for both sticks

        String heading1 = "OpStick1";
        String heading2 = "OpStick2";

        if(opstick1buttons.length() > 0)heading1 = heading1 + " " + "Btn " + opstick1buttons;

        if(opstick2buttons.length() > 0)heading2 = heading2 + " " + "Btn " + opstick2buttons;

        //print out the headings with buttons presses shown
        messageCenter.println(MessageCenter.kOperatorJoystickScreen,
                MessageCenter.kLine1,
                heading1);

        messageCenter.println(MessageCenter.kOperatorJoystickScreen,
                MessageCenter.kLine4,
                heading2);

    }

    //for message center output
    public void outputStatus(){
        displayDriveMode();
        displayDriverInputs();
        displayDriverButtonPresses();
        displayOperatorInputs();
    }

    //for data recorder output
    public void recordStatus(){
        dataRecorder.record("driveStick1X",driveStick1.getX());
        dataRecorder.record("driveStick1Y",driveStick1.getY());
        dataRecorder.record("driveStick1Z",driveStick1.getZ());

        dataRecorder.record("driveStick1mag",driveStick1.getMagnitude());
        dataRecorder.record("driveStick1dir",driveStick1.getDirectionDegrees());
    }
}
